#include "TCBVec3Interpolator.h"

//catmull-rom splines
void TCBVec3Interpolator::GetVectors(float time, float &normalizedTime, Vec3 &pt1, Vec3 &pt2, Vec3 &tn1, Vec3 &tn2)
{
	//key on right side
	std::vector<Vec3Key*>::iterator rk;

	for (rk = keys.begin(); rk != keys.end(); rk++)
		if ((*rk) ->time >= time)
			break;
	
	//time normalization
	normalizedTime = (time - (*(rk - 1)) ->time) / ((*rk) ->time - (*(rk - 1)) ->time);

	pt1 = (*(rk - 1)) ->value;
	pt2 = (*rk) ->value;

	if ((rk - 1) != keys.begin())
		tn1 = (pt2 - (*(rk - 2)) ->value) * 0.5f;
	else
		tn1 = (pt2 - pt1) * 1.5f;

	if ((rk + 1) != keys.end())
		tn2 = ((*(rk + 1)) ->value - pt1) * 0.5f;
	else
		tn2 = ((*rk) ->value - (*(rk - 1)) ->value) * 1.5f;

	if (keys.size() == 2)
		tn1 = tn2 = pt2 - pt1;
	else
	{
		if ((rk - 1) == keys.begin())
			tn1 = tn1 - (tn2 * 0.5f);

		if ((rk + 1) == keys.end())
			tn2 = tn2 - (tn1 * 0.5f);
	}
}

void TCBVec3Interpolator::GetValue(float time, Vec3 &value)
{
	if (keys.size() == 0)
		return;

	if (time <= (*keys.begin()) ->time)
	{
		value = (*keys.begin()) ->value;
		return;
	}

	if (time >= (*(keys.end() - 1)) ->time)
	{
		value = (*(keys.end() - 1)) ->value;
		return;
	}

	Vec3 pt1;
	Vec3 pt2;
	Vec3 tn1;
	Vec3 tn2;
	float normTime;

	GetVectors(time, normTime, pt1, pt2, tn1, tn2);

	float t1 = normTime;
	float t2 = t1 * normTime;
	float t3 = t2 * normTime;

	value =
		(pt1 * (2.0f * t3 - 3.0f * t2 + 1)) +
		(pt2 * (-2.0f * t3 + 3.0f * t2)) +
		(tn1 * (t3 - 2.0f * t2 + t1)) +
		(tn2 * (t3 - t2));
}

void TCBVec3Interpolator::AddKey(float time, Vec3 value)
{
	Vec3Key *key = new Vec3Key();
	key ->time = time;
	key ->value = value;

	std::vector<Vec3Key*>::iterator i;
	for (i = keys.begin(); i != keys.end(); i++)
		if ((*i) ->time >= key ->time)
			break;

	keys.insert(i, key);
}
